www.gusucode.com > Sphero Connectivity Package 程序工具箱matlab源码 > Sphero Connectivity Package/examples/control_sphero.m

    function [speed] = control_sphero(dist, groundspeed, Kp, Ki, Kd, stopRadius, maxspeed, minspeed, restartspeed, clearvars)
%CONTROL_SPHERO PID Controller for Sphero motion
% CONTROL_SPHERO(DIST, GROUNDSPEED, KP, KI, KD, STOPRADIUS, MAXSPEED, MINSPEED, RESTARTSPEED, CLEARVARS)
% Input Arguments:
%   DIST        : Distance of the current point of Sphero from the desired point
%   GROUNDSPEED : Current speed of the Sphero
%   KP          : Proportional controller gain
%   KI          : Integral controller gain
%   KD          : Derivative controller gain
%   STOPRADIUS  : Maximum desired distance from desired point within which the Sphero should try to stop 
%   MAXSPEED    : Maximum saturation speed for the Sphero
%   MINSPEED    : Minimum saturation speed for the Sphero
%   RESTARTSPEED: Minimum speed required to restart the Sphero, if it stops at a point where it is not supposed to. 
%                 This minumum speed is required to get the Sphero to start moving again, due to its inertia
%   CLEARVARS   : Clear the persistent variables in the function
%
% Output Arguments:
%   SPEED       : Speed to be provided to the Sphero
%
% Copyright 2015, The MathWorks, Inc.

%% Define persistent variables
persistent init prevu preve prevt prev2e prev2t

%% Clear variables if appropriate flag received
if clearvars
    clear init prevu preve prevt prev2e prev2t
    return
end

%% Initialize
if isempty(init)
    prevu = 0;
    preve = 0;
    prev2e = 0;
    prev2t = cputime;
    prevt = cputime;
    init = 1;
end

%% PID Control
t = cputime;
dt = t-prevt;
dt2 = prevt-prev2t;

if dist<stopRadius
    u=0;
else
    if dt<eps || dt2<eps
        u = prevu+Kp*(dist-preve)+Ki*dt*dist;
    else
        u = prevu+Kp*(dist-preve)+Ki*dt*dist + Kd*((dist-preve)/dt - (preve-prev2e)/dt2);
    end
    
    if groundspeed<2 && u<restartspeed
        u = restartspeed;
    end
        
end

prevu = u;
prev2e = preve;
preve = dist;
prev2t = prevt;
prevt = t;

%% Saturate
if u>maxspeed
    speed = maxspeed;
elseif u<minspeed
     speed = minspeed;
else
    speed = u;
end


end